#! /usr/bin/env python
# -*- coding: utf-8 -*-
# @Time     2021/9/1 下午1:48
# @Author   chenJiQing
# @Email    1013692111@qq.com
# @File     make_plan.py
# @Software PyCharm

# ! /usr/bin/env python

"""
    需求:
        编写两个节点实现服务通信，客户端节点需要提交两个整数到服务器
        服务器需要解析客户端提交的数据，相加后，将结果响应回客户端，
        客户端再解析

    客户端实现:
        1.导包
        2.初始化 ROS 节点
        3.创建请求对象
        4.发送请求
        5.接收并处理响应

    优化:
        加入数据的动态获取


"""
# 1.导包
import rospy
from nav_msgs.srv import GetPlan, GetPlanRequest, GetPlanResponse
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker


# 初始化RVIZ的路径点可视化标记
def push_markers(marker_pub, points):
    # Set up our waypoint markers
    marker_scale = 0.2
    marker_lifetime = 0  # 0 is forever
    marker_ns = 'waypoints'
    marker_id = 0
    marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}

    # Define a marker publisher.

    # Initialize the marker points list.
    waypoint_markers = Marker()
    waypoint_markers.ns = marker_ns
    waypoint_markers.id = marker_id
    waypoint_markers.type = Marker.CUBE_LIST
    waypoint_markers.action = Marker.ADD
    waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
    waypoint_markers.scale.x = marker_scale
    waypoint_markers.scale.y = marker_scale
    waypoint_markers.color.r = marker_color['r']
    waypoint_markers.color.g = marker_color['g']
    waypoint_markers.color.b = marker_color['b']
    waypoint_markers.color.a = marker_color['a']

    waypoint_markers.header.frame_id = 'map'
    waypoint_markers.header.stamp = rospy.Time.now()
    list_points = []
    for i in range(len(points)):
        list_points.append(points[i].pose.position)
    waypoint_markers.points = list_points
    marker_pub.publish(waypoint_markers)


if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("MakePlan_Client_p")
    marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=10)
    # 3.创建请求对象
    client = rospy.ServiceProxy("/move_base/make_plan", GetPlan)
    # 请求前，等待服务已经就绪
    # rospy.wait_for_service("make_plan")
    rospy.loginfo("等待服务!")
    client.wait_for_service()
    rospy.loginfo("发起服务请求!")
    rate = rospy.Rate(0.1)
    while not rospy.is_shutdown():
        # 4.发送请求,接收并处理响应
        req = GetPlanRequest()
        # Setting goal: Frame:map, Position(1.680, 2.618, 0.000), Orientation(0.000, 0.000, -0.723, 0.691) = Angle: -1.616

        # Setting goal: Frame:map, Position(0.640, 0.526, 0.000), Orientation(0.000, 0.000, 0.290, 0.957) = Angle: 0.588
        start = PoseStamped()
        start.header.frame_id = 'map'
        start.header.stamp = rospy.Time.now()
        start.pose.position.x = 0.640
        start.pose.position.y = 0.526
        start.pose.position.z = 0.0
        start.pose.orientation.x = 0.00
        start.pose.orientation.y = 0.00
        start.pose.orientation.z = 0.290
        start.pose.orientation.w = 0.957
        # Setting goal: Frame:map, Position(1.843, -3.779, 0.000), Orientation(0.000, 0.000, -0.709, 0.706) = Angle: -1.575
        # Setting goal: Frame:map, Position(3.713, 2.942, 0.000), Orientation(0.000, 0.000, 0.435, 0.900) = Angle: 0.901
        # Setting goal: Frame:map, Position(-1.493, 1.534, 0.000), Orientation(0.000, 0.000, -0.136, 0.991) = Angle: -0.272
        goal = PoseStamped()
        goal.header.frame_id = 'map'
        goal.header.stamp = rospy.Time.now()
        goal.pose.position.x = 3.713
        goal.pose.position.y =  2.942
        goal.pose.position.z = 0.0
        goal.pose.orientation.x = 0.00
        goal.pose.orientation.y = 0.00
        goal.pose.orientation.z = 0.435
        goal.pose.orientation.w = 0.900

        # goal.pose.position.x = -1.493
        # goal.pose.position.y = 1.534
        # goal.pose.position.z = 0.0
        # goal.pose.orientation.x = 0.00
        # goal.pose.orientation.y = 0.00
        # goal.pose.orientation.z = -0.136
        # goal.pose.orientation.w = 0.991
        # 参数
        req.start = start
        req.goal = goal
        req.tolerance = int(10)
        resp = client.call(req)
        rospy.loginfo("请求结束!")
        # rospy.loginfo(resp.plan.poses)
        poses = resp.plan.poses
        rospy.loginfo(type(poses))
        rospy.loginfo(len(poses))
        # 显示Poses
        push_markers(marker_pub, poses)
        rate.sleep()
